#ifndef MANUAL_TRAJ_OPTIMIZER
#define MAUNAL_TRAJ_OPTIMIZER

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl_conversions/pcl_conversions.h>

#include "quad_msgs/Trajectory.h"
#include "transform_utils/type_transform.h"
#include "transform_utils/display.h"
#include "trajectory_optimization/trajectory_optimization.h"
#include "trajectory_optimization/bspline_smooth.h"

namespace Planner
{
    using Eigen::MatrixXd;
    using Eigen::Vector3d;
    using std::cout;
    using std::endl;
    using std::string;
    using std::vector;

    class ManualTrajOptimizer
    {
    public:
        ManualTrajOptimizer(const ros::NodeHandle &node_handle);
        ~ManualTrajOptimizer() = default;

        void init();

        int optimize();

        void display();

    private:
        int readData();

        void writeData();

        void simpleSmooth();

        void multiFuncOptimize();

        bool waypointsCheck();

        bool trajectoryCheck();

        double calcDis(double x, double y, double z);

        ros::NodeHandle nh_;
        
        ros::Publisher record_traj_pub_, optimized_traj_pub_;
        ros::Publisher cloud_map_pub_, cloud_map_filtered_pub_;

        BsplineSmooth bspline_smooth_;
        TrajectoryGenerator traj_gen_;
        TrajectoryOptimization traj_opt_;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map_, cloud_map_filtered_;
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_;
        sensor_msgs::PointCloud2 cloud_map_msgs_, cloud_map_filtered_msgs_;

        vector<MatrixXd> coeffs_;
        vector<double> time_seq_;

        vector<Vector3d> waypoints_, waypoints_smoothed_;

        int opt_seg_num_;
        int order_;
        bool optimize_flag_;
        string manual_bagname_, optimize_bagname_;
        string cloud_filename_;
        double init_height_;
        int filter_num_;
        double filter_radius_;
        double des_gap_;
    };

} // namespace Planner

#endif